/**
 * @file	PID.c
 * @purpose
 * @brief
 * @version
 * @date	28-sep-2009
 * @author	nlv15840
*/
/*----------------------------------------------------------------------------
 * Software that is described herein is for illustrative purposes only
 * which provides customers with programming information regarding the
 * products. This software is supplied "AS IS" without any warranties.
 * NXP Semiconductors assumes no responsibility or liability for the
 * use of the software, conveys no license or title under any patent,
 * copyright, or mask work right to the product. NXP Semiconductors
 * reserves the right to make changes in the software without
 * notification. NXP Semiconductors also make no representation or
 * warranty that such application will be suitable for the specified
 * use without further testing or modification.
 **********************************************************************/
#include "Application.h"
#include "debug_frmwrk.h"
void PID_calc_cntr( PIDstr *ptr )
{
 
    static short int u[2];
    float a0,a1,a2;
    uint8_t Ts=100;
    	a0 = ptr->p + ptr->i*Ts/1000/2 + ptr->d*1000/Ts;	//Calculate PID parameters
	a1 = -ptr->p + ptr->i*Ts/1000/2 - 2*ptr->d*1000/Ts;
	a2 = ptr->d*1000/Ts;
     ptr->err[2] = ptr->sp -ptr->RPM;
     u[1] = u[0] + a0*ptr->err[2] + a1*ptr->err[1] + a2*ptr->err[0];	 
 //   _DBD16(ptr->err[0]);
 //     _DBG("\n");
    if(u[1]<0)
    {
        u[1]=0;
    }
    if(u[1]>2000)
    {
        u[1]=1950;
    }
	u[0] = u[1];
	ptr->err[0] = ptr->err[1];
	ptr->err[1] = ptr->err[2];
        	ptr->mv=(WORD)u[1];
}

